#ifndef __BUNDLEADJUSTMENT_H__
#define __BUNDLEADJUSTMENT_H__

#include "sba.h"

class BundleAdjuster {
	private:
		struct globs_{
			double *rot0params; /* initial rotation parameters, combined with a local rotation parameterization */
			double *intrcalib; /* the 5 intrinsic calibration parameters in the order [fu, u0, v0, ar, skew],
      		                * where ar is the aspect ratio fv/fu.
      		                * Used only when calibration is fixed for all cameras;
      		                * otherwise, it is null and the intrinsic parameters are
      		                * included in the set of motion parameters for each camera
      		                */
  		int nccalib; /* number of calibration parameters that must be kept constant.
      		          * 0: all parameters are free 
      		          * 1: skew is fixed to its initial value, all other parameters vary (i.e. fu, u0, v0, ar) 
      		          * 2: skew and aspect ratio are fixed to their initial values, all other parameters vary (i.e. fu, u0, v0)
      		          * 3: meaningless
      		          * 4: skew, aspect ratio and principal point are fixed to their initial values, only the focal length varies (i.e. fu)
      		          * 5: all intrinsics are kept fixed to their initial values
      		          * >5: meaningless
      		          * Used only when calibration varies among cameras
      		          */

  		int ncdist; /* number of distortion parameters in Bouguet's model that must be kept constant.
      		         * 0: all parameters are free 
      		         * 1: 6th order radial distortion term (kc[4]) is fixed
      		         * 2: 6th order radial distortion and one of the tangential distortion terms (kc[3]) are fixed
      		         * 3: 6th order radial distortion and both tangential distortion terms (kc[3], kc[2]) are fixed [i.e., only 2nd & 4th order radial dist.]
      		         * 4: 4th & 6th order radial distortion terms and both tangential distortion ones are fixed [i.e., only 2nd order radial dist.]
      		         * 5: all distortion parameters are kept fixed to their initial values
      		         * >5: meaningless
      		         * Used only when calibration varies among cameras and distortion is to be estimated
      		         */
  		int cnp, pnp, mnp; /* dimensions */

			double *ptparams; /* needed only when bundle adjusting for camera parameters only */
			double *camparams; /* needed only when bundle adjusting for structure parameters only */
		} globs;
		
		static struct sba_crsm measurement;
		static struct sba_crsm jacobian;	

	public:
		inline static void quatMultFast(double q1[FULLQUATSZ], double q2[FULLQUATSZ], double p[FULLQUATSZ]);

		static void img_projsKRT_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *hx, void *adata);

		static void img_projsKRT_jac_x(double *p, struct sba_crsm *idxij, int *rcidxs, int *rcsubs, double *jac, void *adata);

		static void bundleAdjust();

		static void addImageParam()s;

};

#endif
